////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2012 Kentaro Sekimoto  All rights reserved.
////////////////////////////////////////////////////////////////////////////

#include <tinyhal.h>
#include <Include\USB_decl.h>
#include <pal\com\usb\USB.h>
#include "..\FM4.h"
#include "FM3_usb_functions.h"

USB_CONTROLLER_STATE g_FM3_USB_ControllerState;
Hal_Queue_KnownSize<USB_PACKET64,USB_QUEUE_PACKET_COUNT> QueueBuffers[USB_MAX_EP - 1];

static UINT8 FM3_USB_EP0Buffer[MAX_EP0_SIZE];
static UINT16 FM3_USB_EndpointStatus[USB_MAX_EP];
static BOOL FM3_USB_PinsProtected = FALSE;
static UINT8 FM3_USB_PreviousDeviceState;
static UINT16 FM3_USB_EP_Type = 0;

#ifdef STATE_DUMP
void State_Dump(USB_CONTROLLER_STATE *State)
{
    USB_DEBUG2("Initialized %d\n", State->Initialized);
    USB_DEBUG2("PacketSize %d\n", State->PacketSize);
    USB_DEBUG2("ResidualCount %d\n", State->ResidualCount);
    USB_DEBUG2("Expected %d\n", State->Expected);
}
#endif

static void wait(volatile UINT32 count)
{
    while (count-- > 0) ;
}

void FM3_USB_Clock_Initialize(void)
{
    REG_W(bFM3_CLK_GATING_CKEN2_USBCK0, BIT_SET);
    /* disable USB clock */
    REG_W(bFM3_USBETHERNETCLK_UCCR_UCEN0, BIT_CLR);
    /* wait for USB clock stop */
    while (REG_R(bFM3_USBETHERNETCLK_UCCR_UCEN0) != BIT_CLR) ;
    /* disable USB-PLL clock */
    REG_W(bFM3_USBETHERNETCLK_UPCR1_UPLLEN, BIT_CLR);
    /* select PLL macro clock */
    REG_W(bFM3_USBETHERNETCLK_UCCR_UCSEL0, BIT_SET);
    /* select main clock as input clock */
    REG_W(bFM3_USBETHERNETCLK_UPCR1_UPINC, BIT_CLR);
    /* select clock stabilization time */
    REG_W(FM3_USBETHERNETCLK->UPCR2, UPCR2_UPOWT_INIT_VALUE);
    /* USB-PLL=Fin*N/K -> 96MHz=4MHz*24/1 */
    /* USB-PLL clock configuration register(K) initialize */
    REG_W(FM3_USBETHERNETCLK->UPCR3, UPCR3_UPLLK_INIT_VALUE);        // 4MHz:0, 16MHz:0
    /* USB-PLL clock configuration register(N) initialize */
    REG_W(FM3_USBETHERNETCLK->UPCR4, UPCR4_UPLLN_INIT_VALUE);        // 4MHz:60, 16Mz:24
    /* USB-PLL clock configuration register(N) initialize */
    REG_W(FM3_USBETHERNETCLK->UPCR5, UPCR5_UPLLN_INIT_VALUE);        // 4MHz:5,  16Mz:6
    /* USB-PLL clock stabilize interrupt disable  */
    REG_W(bFM3_USBETHERNETCLK_UPINT_ENR_UPCSE, BIT_CLR);
    /* enable USB-PLL clock */
    REG_W(bFM3_USBETHERNETCLK_UPCR1_UPLLEN, BIT_SET);
    /* wait for USB-PLL clock ready */
    while (REG_R(bFM3_USBETHERNETCLK_UP_STR_UPRDY) == BIT_CLR) ;
    /* enable USB clock */
    REG_W(bFM3_USBETHERNETCLK_UCCR_UCEN0, BIT_SET);
    /* wait for 5 cycle */
    __NOP();
    __NOP();
    __NOP();
    __NOP();
    __NOP();
    /* enable USB controller */
    REG_W(bFM3_USBETHERNETCLK_USBEN0_USBEN0, BIT_SET);
    return;
}

void FM3_USB_Port_Initialize(void)
{
    REG_W(bFM3_GPIO_PFR6_P60, 0);    // P60 GPIO
    REG_W(bFM3_GPIO_DDR6_P60, 0);    // P60 input for checking VBUS
    REG_W(bFM3_GPIO_DDR6_P61, 1);    // P61 outout for controlling pullup.
}

void FM3_USB_Driver_EP0_Initialize(void)
{
    FM3_USB0_EP0_TxInt_Disable();   // disable Tx interrupt
    FM3_USB0_EP0_RxInt_Enable();    // enable Rx interrupt
}

void FM3_USB_Driver_RxFIFO_Init(void)
{
    REG_W(bFM3_USB0_EP0OS_BFINI, 1);
    REG_W(bFM3_USB0_EP0OS_BFINI, 0);
}

void FM3_USB_Driver_TxFIFO_Init(void)
{
    REG_W(bFM3_USB0_EP0IS_BFINI, 1);
    REG_W(bFM3_USB0_EP0IS_BFINI, 0);
}

void FM3_USB_Driver_SetStall(void)
{
    FM3_USB_Driver_RxFIFO_Init();
    FM3_USB_Driver_TxFIFO_Init();
    FM3_USB0_EP0_Stall_Set();
}

void FM3_USB_SetRxStatus(int ep, int status)
{
    volatile UINT16 *pepns = (volatile UINT16 *)(FM3_USB0_EP0OS_ADDR + (ep << 2));
    switch (status) {
    case USB_EP_RX_DIS:
        *pepns &= ~(EPNS_DRQIE | EPNS_SPKIE);
        break;
    case USB_EP_RX_STALL:
    {
        volatile UINT16 *pepnc = (volatile UINT16 *)(FM3_USB0_EP0C_ADDR + (ep << 2));
        *pepnc |= EPNC_STAL;
    }
    break;
    case USB_EP_RX_NAK:
        break;
    default:    // USB_EP_RX_VALID
        *pepns |= (EPNS_DRQIE | EPNS_SPKIE);
        break;
    }
}

#if 0
void FM3_USB_SetTxStatus(int ep, int status)
{
    volatile UINT16 *pepnc = (volatile UINT16 *)(FM3_USB0_EP0C_ADDR + (ep << 2));
    volatile UINT16 *pepns;
    if (ep == 0)
        pepns = (volatile UINT16 *)(FM3_USB0_EP0IS_ADDR);
    else
        pepns = (volatile UINT16 *)(FM3_USB0_EP0OS_ADDR + (ep << 2));
    switch (status) {
    case USB_EP_TX_DIS:
        *pepns = (*pepns) & (~(EPNS_DRQIE));
        break;
    case USB_EP_TX_STALL:
        *pepnc = *pepnc | EPNC_STAL;
        break;
    case USB_EP_TX_NAK:
        break;
    default:    // USB_EP_TX_VALID
        *pepns = (*pepns) | (EPNS_DRQIE);
        break;
    }
}
#endif

/*
 * Suspend Event Interrupt Handler
 */
void FM3_USB_Driver_SuspendEvent (USB_CONTROLLER_STATE* State)
{
    USB_DEBUG1("S");
    FM3_USB0_Suspend_Clear();
    FM3_USB_PreviousDeviceState = State->DeviceState;
    State->DeviceState = USB_DEVICE_STATE_SUSPENDED;
    USB_StateCallback(State);
}

/*
 * Resume Event Interrupt Handler
 */
void FM3_USB_Driver_ResumeEvent(USB_CONTROLLER_STATE* State)
{
    UINT16 dummy;
    USB_DEBUG1("W");    // Wakeup
    wait(10000);        // 10ms
    if (State->DeviceState == USB_DEVICE_STATE_SUSPENDED) {
        FM3_USB0_Suspend_Clear();       // clear suspend
        dummy = FM3_USB0->UDCC;         // dummy read
        REG_W(bFM3_USB0_UDCC_USTP, 0);  // normal mode
        dummy = FM3_USB0->UDCC;         // dummy read
        FM3_USB0_Suspend_Enable();      // enable suspend
        State->DeviceState = FM3_USB_PreviousDeviceState;
        USB_StateCallback(State);
    }
    FM3_USB0_Wakeup_Clear();            // clear wakeup event
}

/*
 * Reset Event Interrupt Handler
 */
void FM3_USB_Driver_ResetEvent(USB_CONTROLLER_STATE* State)
{
    USB_DEBUG1("!\n");
    // ToDo Reset
    REG_W(bFM3_USB0_UDCS_BRST, 0);      // clear bus reset event
    FM3_USB_Driver_TxFIFO_Init();
    FM3_USB_Driver_RxFIFO_Init();
    USB_ClearEvent(0, USB_EVENT_ALL);
//    State->FirstGetDescriptor = TRUE;
    State->DeviceState = USB_DEVICE_STATE_DEFAULT;
    State->Address = 0;
    USB_StateCallback(State);
}

void FM3_USB_Driver_WriteTxFIFO(UINT32 endpoint, UINT8 *buf, UINT32 size)
{
    GLOBAL_LOCK(irq);
    volatile UINT8 *pfifol = (volatile UINT8 *)(FM3_USB0_EP0DTL_ADDR + (endpoint << 2));
    volatile UINT8 *pfifoh = (volatile UINT8 *)(FM3_USB0_EP0DTH_ADDR + (endpoint << 2));
    int n;
    for (n = size; n >= 2; n -= 2) {
        *pfifol = *buf;
        buf++;
        *pfifoh = *buf;
        buf++;
    }
    if (n == 1) {
        *pfifol = *buf;
    }
}

void FM3_USB_Driver_ReadRxFIFO(UINT32 endpoint, UINT8 *buf, UINT32 size)
{
    GLOBAL_LOCK(irq);
    volatile UINT8 *pfifol = (volatile UINT8 *)(FM3_USB0_EP0DTL_ADDR + (endpoint << 2));
    volatile UINT8 *pfifoh = (volatile UINT8 *)(FM3_USB0_EP0DTH_ADDR + (endpoint << 2));
    int n;
    for (n = size; n > 0; n -= 2) {
        *buf = *pfifol;
        buf++;
        *buf = *pfifoh;
        buf++;
    }
    if (n == 1) {
        *buf = *pfifol;
    }
}

void FM3_USB_Driver_EP_In_Int(USB_CONTROLLER_STATE* State, int endpoint)
{
    ASSERT_IRQ_MUST_BE_OFF();
    if (State->Queues[endpoint] != NULL && State->IsTxQueue[endpoint]) {
        USB_PACKET64* Packet64 = USB_TxDequeue(State, endpoint, TRUE);
        if (Packet64) {
#ifdef USB_DUMMY_READ
            volatile UINT16 *pepnc = (volatile UINT16 *)(FM3_USB0_EP0C_ADDR + (endpoint << 2));
#endif
            volatile UINT16 *pepns = (volatile UINT16 *)(FM3_USB0_EP0OS_ADDR + (endpoint << 2));
            UINT32 size = Packet64->Size;
            USB_DEBUG2("i%d\n", size);
            //lcd_printf("i%d ", size);
            wait(100000);    // if there is no dummy loop, connection with Visual Studio hangs up.
            FM3_USB_Driver_WriteTxFIFO(endpoint, (UINT8 *)Packet64->Buffer, size);
            (*pepns) &= ~(EPNS_DRQ);
#ifdef USB_DUMMY_READ
            UINT16 dummy = *pepnc;
#endif
#ifdef USB_DEBUG
            //usb_memory_dump((unsigned long)Packet64->Buffer, size);
#endif
        }
    }
}

void FM3_USB_Driver_EP_Out_Int (USB_CONTROLLER_STATE* State, int endpoint)
{
    ASSERT_IRQ_MUST_BE_OFF();
    BOOL full;
#ifdef USB_DUMMY_READ
    volatile UINT16 *pepnc = (volatile UINT16 *)(FM3_USB0_EP0C_ADDR + (endpoint << 2));
#endif
    volatile UINT16 *pepns = (volatile UINT16 *)(FM3_USB0_EP0OS_ADDR + (endpoint << 2));
    USB_PACKET64* Packet64 = USB_RxEnqueue(State, endpoint, full);
    if (Packet64) {
        UINT32 size;
        if (endpoint == 1)
            size = *pepns & 0x01FF;
        else
            size = *pepns & 0x007F;
        FM3_USB_Driver_ReadRxFIFO(endpoint, (UINT8 *)Packet64->Buffer, size);
        (*pepns) &= ~(EPNS_DRQ | EPNS_SPK);
#ifdef USB_DUMMY_READ
        UINT16 dummy = *pepnc;
#endif
        Packet64->Size = size;
        if (!full) {
            // reenable Rx
            USB_DEBUG2("o%d\n", size);
            //lcd_printf("o%d ", size);
            FM3_USB_SetRxStatus(endpoint, USB_EP_RX_VALID);
        } else {
            USB_DEBUG2("v%d\n", size);
            //lcd_printf("v%d ", size);
            FM3_USB_SetRxStatus(endpoint, USB_EP_RX_DIS);
        }
#ifdef USB_DEBUG
        //usb_memory_dump((unsigned long)Packet64->Buffer, size);
#endif
    } else {
        // should not happen
        USB_DEBUG1("?");
    }
}

void FM3_USB_Driver_CtrlTrans_Setup_Receive_Process(USB_CONTROLLER_STATE *State)
{
    volatile UINT16 *ep0os = (volatile UINT16 *)(FM3_USB0_EP0OS_ADDR);
    UINT32 size = *ep0os & 0x007F;
    if (size > 0) {
        USB_DEBUG2("O%d", size);
    } else {
        USB_DEBUG2("O%d", size);
    }
    FM3_USB_Driver_ReadRxFIFO(0, (UINT8 *)&FM3_USB_EP0Buffer[0], size);
    FM3_USB0_EP0_RxInt_Clear();     // clear Rx interrupt
    FM3_USB0_EP0_SPK_Clear();
#ifdef USB_DUMMY_READ
    UINT16 dummy = FM3_USB0->EP0C;
#endif
    State->Data = (UINT8 *)&FM3_USB_EP0Buffer[0];
    State->DataSize = (UINT8)size;
    USB_SETUP_PACKET* Setup;
    Setup = (USB_SETUP_PACKET*)State->Data;
    UINT8 result = USB_ControlCallback(State);
    switch(result) {
    case USB_STATE_DATA:
        /* setup packet was handled and the upper layer has data to send */
        USB_DEBUG1("D");
        FM3_USB0_EP0_TxInt_Enable();
        break;
    case USB_STATE_ADDRESS:
        /* upper layer needs us to change the address */
        // address change delayed to end of reply packet
        break;
    case USB_STATE_DONE:
        USB_DEBUG1("E\n");
        State->DataCallback = NULL;
        break;
    case USB_STATE_STALL:
        // setup packet failed to process successfully
        // set stall condition on the control endpoint
        FM3_USB_SetRxStatus(0, USB_EP_RX_STALL);
        USB_DEBUG1("l");
        // ********** skip rest of function **********
        return;
    case USB_STATE_STATUS:
        break;
    case USB_STATE_CONFIGURATION:
        break;
    case USB_STATE_REMOTE_WAKEUP:
        // It is not using currently as the device side won't go into SUSPEND mode unless
        // the PC is purposely to select it to SUSPEND, as there is always SOF in the bus
        // to keeping the device from SUSPEND.
        USB_DEBUG1("w");
        break;
    default:
        break;
    }
}

void FM3_USB_Driver_CtrlTrans_Process(USB_CONTROLLER_STATE *State, UINT8 type)
{
    switch((int)type) {
    case CTRLTRANS_SETUP_DETECTED:
    case CTRLTRANS_DATA_RECEIVED:
        // setup
        FM3_USB_Driver_CtrlTrans_Setup_Receive_Process(State);
        break;
    case CTRLTRANS_DATA_SENDED:
        // send
        if (State->DataCallback != NULL) {
            State->DataCallback(State);  // this call can't fail
            UINT32 size = State->DataSize;
            USB_DEBUG2("I%d", size);
#ifdef USB_DEBUG
            //usb_memory_dump((unsigned long)State->Data, size);
#endif
            if (size > 0) {
                FM3_USB_Driver_WriteTxFIFO(0, (UINT8 *)State->Data, size);
                FM3_USB0_EP0_TxInt_Clear();
#ifdef USB_DUMMY_READ
                UINT16 dummy = FM3_USB0->EP0C;
#endif
            } else {
                USB_DEBUG1("N");
                FM3_USB0_EP0_TxInt_Clear();
#ifdef USB_DUMMY_READ
                UINT16 dummy = FM3_USB0->EP0C;
#endif
            }
            if (State->Expected != 0)
                FM3_USB0_EP0_TxInt_Enable();
            else
                FM3_USB0_EP0_TxInt_Disable();
        } else {
            if (State->CurrentState == USB_DEVICE_STATE_ADDRESS) {
                USB_DEBUG1("A");
            }
        }
        break;
    }
#if 1
    for (int ep = 1; ep < USB_MAX_EP; ep++) {
        if (State->Queues[ep] && State->IsTxQueue[ep])
            FM3_USB_Driver_EP_In_Int(State, ep);
    }
#endif
}

void FM3_USB_Driver_ConfigEvent(USB_CONTROLLER_STATE* State)
{
    USB_DEBUG1("C\n");
    REG_W(bFM3_USB0_UDCS_CONF, 0);
    State->ConfigurationNum = ((uint8_t)REG_R(bFM3_USB0_UDCIE_CONFN));
    State->DeviceState = USB_DEVICE_STATE_CONFIGURED;
    USB_StateCallback(State);
}

// ep0
void FM3_USB_Interrupt(void* param) __attribute__ ((interrupt));
void FM3_USB_Interrupt(void* param)
{
#ifdef USB_DEBUG
    UINT16 udcc = FM3_USB0->UDCC;
    UINT16 ep0c = FM3_USB0->EP0C;
    UINT8 udcs = FM3_USB0->UDCS;
    UINT8 udcie = FM3_USB0->UDCIE;
    UINT16 ep0is = FM3_USB0->EP0IS;
    UINT16 ep0os = FM3_USB0->EP0OS;
    //usb_debug_printf("%02x %02x %02x %02x %02x %02x\n", udcc, ep0c, udcs, udcie, ep0is, ep0os);
#endif
    // EP0
    INTERRUPT_START;
    USB_CONTROLLER_STATE *State = &g_FM3_USB_ControllerState;
    //FM3_USB_Driver_Event(State);
    if (REG_R(bFM3_USB0_EP0OS_DRQO) && REG_R(bFM3_USB0_EP0OS_DRQOIE) == TRUE) {
        UINT8 type = CTRLTRANS_DATA_RECEIVED;
        if (REG_R(bFM3_USB0_UDCS_SETP)) {
            REG_W(bFM3_USB0_UDCS_SETP, 0);
            type = CTRLTRANS_SETUP_DETECTED;
        }
        FM3_USB_Driver_CtrlTrans_Process(State, type);
    } else if (REG_R(bFM3_USB0_EP0IS_DRQI) && REG_R(bFM3_USB0_EP0IS_DRQIIE)) {
        FM3_USB_Driver_CtrlTrans_Process(State, CTRLTRANS_DATA_SENDED);
    } else if (REG_R(bFM3_USB0_UDCS_CONF) && REG_R(bFM3_USB0_UDCIE_CONFIE)) {
        FM3_USB_Driver_ConfigEvent(State);
    } else if (REG_R(bFM3_USB0_UDCS_BRST) && REG_R(bFM3_USB0_UDCIE_BRSTIE)) {
        FM3_USB_Driver_ResetEvent(State);
    } else if (REG_R(bFM3_USB0_UDCS_SUSP) && REG_R(bFM3_USB0_UDCIE_SUSPIE)) {
        FM3_USB_Driver_SuspendEvent(State);
    } else if (REG_R(bFM3_USB0_UDCS_WKUP) && REG_R(bFM3_USB0_UDCIE_WKUPIE)) {
        FM3_USB_Driver_ResumeEvent(State);
    } else {
        //USB_DEBUG3("J%04x %04x\n", ep0is, ep0os);
        //USB_DEBUG3("j%02x %02x\n", udcs, udcie);
    }
    INTERRUPT_END;
}

// ep1 - ep5
void FM3_USBF_Interrupt(void* param) __attribute__ ((interrupt));
void FM3_USBF_Interrupt(void* param)
{
    // EP1 - EP5
    INTERRUPT_START;
    USB_CONTROLLER_STATE *State = &g_FM3_USB_ControllerState;
    volatile UINT16 *pepns;
    //volatile UINT16 *pepnc;
    for (int endpoint = 1; endpoint < USB_MAX_EP; endpoint++) {
        pepns = (volatile UINT16 *)(FM3_USB0_EP0OS_ADDR + (endpoint << 2));
        //pepnc = (volatile UINT16 *)(FM3_USB0_EP0C_ADDR + (endpoint << 2));
        if (((*pepns & EPNS_DRQ) != 0) && ((*pepns & EPNS_DRQIE) != 0)) {
            if (State->IsTxQueue[endpoint]) {
                // send
                FM3_USB_Driver_EP_In_Int(State, endpoint);
            } else {
                // receive
                FM3_USB_Driver_EP_Out_Int(State, endpoint);
            }
        }
    }
    INTERRUPT_END;
}

USB_CONTROLLER_STATE * CPU_USB_GetState(int Controller)
{
    if (Controller != 0)
        return NULL;
    return (USB_CONTROLLER_STATE *)&g_FM3_USB_ControllerState;
}

HRESULT CPU_USB_Initialize(int Controller)
{
#ifdef USB_DEBUG
    USART_Initialize( ConvertCOM_ComPort(USART_DEFAULT_PORT), HalSystemConfig.USART_DefaultBaudRate, USART_PARITY_NONE, 8, USART_STOP_BITS_ONE, USART_FLOW_NONE );
#endif
    USB_DEBUG1("CPU_USB_Initialize START\n");
    USB_CONTROLLER_STATE *State = &g_FM3_USB_ControllerState;
    if (Controller != 0)
        return S_FALSE;
    REG_W(bFM3_GPIO_SPSR_USB0C, 1);
    FM3_USB_Clock_Initialize();
    FM3_USB_Port_Initialize();
    FM3_USB_PinsProtected = FALSE;
    FM3_USB0_HostDisconnect();
    FM3_USB_Pullup_Disconnect();
    CPU_USB_ProtectPins(Controller, TRUE);;
    //wait(60000000);
    State->DeviceState = USB_DEVICE_STATE_DETACHED;
    USB_StateCallback(State);
    FM3_USB_PinsProtected = TRUE;
    FM3_USB0_Function_Reset();
    __NOP();
    __NOP();
    __NOP();
#if 1
    REG_W(bFM3_USB0_EP0IS_BFINI, 1);
    REG_W(bFM3_USB0_EP0OS_BFINI, 1);
    REG_W(bFM3_USB0_EP1S_BFINI, 1);
    REG_W(bFM3_USB0_EP2S_BFINI, 1);
    REG_W(bFM3_USB0_EP3S_BFINI, 1);
    REG_W(bFM3_USB0_EP4S_BFINI, 1);
    REG_W(bFM3_USB0_EP5S_BFINI, 1);
#endif
    FM3_USB0_StallClearEnable();
    FM3_USB0_RateFeedbackMode();
    FM3_USB0_BusPower();
    FM3_USB0_Resume_Clear();
    FM3_USB0->EP0C = MAX_EP0_SIZE;
    GLOBAL_LOCK(irq);
    State->EndpointCount = USB_MAX_EP;
    State->EndpointStatus = &FM3_USB_EndpointStatus[0];
    State->PacketSize = MAX_EP0_SIZE;
    int endpoint;
    for (endpoint = 1; endpoint < USB_MAX_EP; endpoint++) {
        State->IsTxQueue[endpoint] = FALSE;
        State->MaxPacketSize[endpoint] = USB_MAX_DATA_PACKET_SIZE;  // 64
    }
    FM3_USB_EP_Type = 0;
    const USB_ENDPOINT_DESCRIPTOR *pep = (USB_ENDPOINT_DESCRIPTOR *)NULL;
    const USB_INTERFACE_DESCRIPTOR *pif  = (USB_INTERFACE_DESCRIPTOR *)NULL;
    while (USB_NextEndpoint(State, pep, pif)) {
        endpoint = pep->bEndpointAddress & 0x7F;
        int endpointSize = pep->wMaxPacketSize;
        State->MaxPacketSize[endpoint] = endpointSize;
        QueueBuffers[endpoint-1].Initialize();
        State->Queues[endpoint] = &QueueBuffers[endpoint-1];
        volatile UINT16 *pepnc = (volatile UINT16 *)(FM3_USB0_EP0C_ADDR + (endpoint << 2));
        volatile UINT16 *pepns;
        if (endpoint == 0)
            pepns= (volatile UINT16 *)(FM3_USB0_EP0IS_ADDR);
        else
            pepns= (volatile UINT16 *)(FM3_USB0_EP0OS_ADDR + (endpoint << 2));
        if (endpoint != 0) {
            if (pep->bEndpointAddress & 0x80) {
                // Tx IN endpoint
                State->IsTxQueue[endpoint] = TRUE;
                //*pepnc &= ~(0x701F);          // clear TYPE, DIR and Size
                *pepnc = 0;                     // clear TYPE, DIR and Size
                *pepnc |= (EPNC_EPEN | EPNC_TYPE_BULK | EPNC_DIR | EPNC_NULE | endpointSize);
                *pepns &= ~EPNS_DRQIE;
                //*pepns |= EPNS_DRQIE;
            } else {
                // Rx OUT endpoint
                //*pepnc &= ~(0x701F);          // clear TYPE, DIR and Size
                *pepnc = 0;                     // clear TYPE, DIR and Size
                *pepnc |= (EPNC_EPEN | EPNC_TYPE_BULK | EPNC_NULE | endpointSize);
                //*pepns |= (EPNS_DRQIE | EPNS_SPKIE) ;
                *pepns |= (EPNS_DRQIE) ;
            }
        }
        FM3_USB_EP_Type |= (pep->bmAttributes & 3) << (endpoint * 2);
    }
    if (FM3_USB_EP_Type == 0)
        return S_FALSE;
    HAL_Time_Sleep_MicroSeconds(1000);
    /* USB function(endpoint 1 ~ 5 DRQ)interrupt level setting */
    CPU_INTC_ActivateInterrupt(USB0_IRQn,  FM3_USBF_Interrupt, 0);
    //CPU_INTC_Priority(USB0F_IRQn, USBF_INTERRUPT_LEVEL);
    /* USB function(endpoint 0  DRQI, DRQO and each status)interrupt level setting */
    CPU_INTC_ActivateInterrupt(USB0_HOST_IRQn,  FM3_USB_Interrupt, 0);
    //CPU_INTC_Priority(USB0F_USB0H_IRQn, USBF_INTERRUPT_LEVEL);
#ifdef STATE_DUMP
    State_Dump(State);
#endif
    FM3_USB0_Function_Start();
    REG_W(bFM3_USB0_EP0IS_BFINI, 1);
    REG_W(bFM3_USB0_EP0OS_BFINI, 1);
    REG_W(bFM3_USB0_EP0IS_BFINI, 0);
    REG_W(bFM3_USB0_EP0OS_BFINI, 0);
    REG_W(bFM3_USB0_EP1S_BFINI, 0);
    REG_W(bFM3_USB0_EP2S_BFINI, 0);
    REG_W(bFM3_USB0_EP3S_BFINI, 0);
    REG_W(bFM3_USB0_EP4S_BFINI, 0);
    REG_W(bFM3_USB0_EP5S_BFINI, 0);
    while (REG_R(bFM3_GPIO_PDIR6_P60) == 0) ;
    REG_W(bFM3_USB0_UDCC_RESUM, FALSE);         // FALSE: clear USB resume, TRUE: set USB resume
    REG_W(bFM3_USB0_UDCIE_BRSTIE, 1);
    REG_W(bFM3_USB0_UDCIE_CONFIE, 1);
    FM3_USB0_Suspend_Enable();
    FM3_USB0_Wakeup_Enable();
    FM3_USB0_EP0_RxInt_Enable();
    FM3_USB0_HostConnect();
    CPU_USB_ProtectPins(Controller, FALSE);
    USB_DEBUG1("*");
    return S_OK;
}

HRESULT CPU_USB_Uninitialize(int Controller)
{
    USB_CONTROLLER_STATE *State = &g_FM3_USB_ControllerState;
    GLOBAL_LOCK(irq);
    /* USB function(endpoint 1 ~ 5 DRQ)interrupt level setting */
    CPU_INTC_DeactivateInterrupt(USB0_IRQn);
    /* USB function(endpoint 0  DRQI, DRQO and each status)interrupt level setting */
    CPU_INTC_DeactivateInterrupt(USB0_HOST_IRQn);
    memset(&g_FM3_USB_ControllerState, 0, sizeof(g_FM3_USB_ControllerState));
#if 0
    REG_W(bFM3_USB0_UDCC_USTP, TRUE);    // FALSE: normal, TRUE: USB operation block stop
    /* disable USB clock */
    REG_W(bFM3_USBCLK_UCCR_UCEN0, BIT_CLR);
    /* wait for USB clock stop */
    while (REG_R(bFM3_USBCLK_UCCR_UCEN0) != BIT_CLR) ;
    /* disable USB-PLL clock */
    REG_W(bFM3_USBCLK_UPCR1_UPLLEN, BIT_CLR);
#endif
    FM3_USB0_HostDisconnect();
    FM3_USB_Pullup_Disconnect();
    CPU_USB_ProtectPins(Controller, TRUE);
    FM3_USB_EP_Type = 0;
    State->DeviceState = USB_DEVICE_STATE_DETACHED;
    return S_OK;
}

BOOL CPU_USB_StartOutput(USB_CONTROLLER_STATE* State, int endpoint)
{
    if (State == NULL || endpoint >= USB_MAX_EP)
        return FALSE;
    GLOBAL_LOCK(irq);
    FM3_USB_Driver_EP_In_Int(State, endpoint);
    return TRUE;
}

BOOL CPU_USB_RxEnable(USB_CONTROLLER_STATE* State, int endpoint)
{
    GLOBAL_LOCK(irq);
    // If this is not a legal Rx queue
    if (State == NULL || State->Queues[endpoint] == NULL || State->IsTxQueue[endpoint])
        return FALSE;
    // enable Rx
    //USB_DEBUG1("q\n");
    FM3_USB_SetRxStatus(endpoint, USB_EP_RX_VALID);
    return TRUE;
}

BOOL CPU_USB_GetInterruptState()
{
    BOOL flag = FALSE;
    volatile UINT16 *pepns;
    for (int endpoint = 1; endpoint < USB_MAX_EP; endpoint++) {
        pepns= (volatile UINT16 *)(FM3_USB0_EP0OS_ADDR + (endpoint << 2));
        if ((*pepns & (EPNS_DRQIE | EPNS_DRQ)) == (EPNS_DRQIE | EPNS_DRQ))
            flag |= TRUE;
    }
    //USB_DEBUG2("Q%d\n", flag);
    return flag;
}

// Puts USB pins into a known state to prevent spurious inputs.
// Parameters
// On
// A value you can set to true to protect the USB pins; set it to false to disable protection.
// Return Value
// true if the operation succeeded; otherwise, false.
//
// Setting the On parameter to true detaches the USB module from the host.
// This protects the USB pins by stopping all USB communications.
// Setting On to false enables the pins, thus enabling the host to detect the USB client.
// As soon as the USB client is detected, the enumeration process begins.
// USB communications resume when the enumeration process has finished.
BOOL CPU_USB_ProtectPins(int Controller, BOOL On)
{
    USB_CONTROLLER_STATE *State = &g_FM3_USB_ControllerState;
    GLOBAL_LOCK(irq);
    if (On == TRUE) {
        if (FM3_USB_PinsProtected == FALSE) {
            USB_DEBUG1("+");
            FM3_USB_PinsProtected = TRUE;
            FM3_USB_Pullup_Disconnect();
            //FM3_USB0_HostDisconnect();
            for(int ep = 1; ep < USB_MAX_EP; ep++) {
                if (State->Queues[ep] && State->IsTxQueue[ep]) {
                    while (USB_TxDequeue(State, ep, TRUE) != NULL);
                }
            }
            State->DeviceState = USB_DEVICE_STATE_DETACHED;
            USB_StateCallback(State);
        }
    } else {
        if (FM3_USB_PinsProtected == TRUE) {
            USB_DEBUG1("-");
            FM3_USB_PinsProtected = FALSE;
            FM3_USB_Pullup_Connect();
            //FM3_USB0_HostConnect();
            State->DeviceState = USB_DEVICE_STATE_ATTACHED;
            USB_StateCallback(State);
        }
    }
    return TRUE;
}
